/*******************************************************************************
*   Copyright (C) Harbin Institute of Technology, Shenzhen.
*				  All Rights Reserved.
*   Department: Decoration
********************************************************************************
* File Name   : control.c
* Author      : Dayuan
* Version     : v0.01
* Date        : 2019/9/6
* Description : Decoration Rototics Manipulator task.
*******************************************************************************/
/*-History----------------------------------------------------------------------
* Version   Date      Name        Changes and comments
* v0.01     2019/9/6  Dayuan    initial version
*=============================================================================*/
/* System Includes -----------------------------------------------------------*/
#include <stdlib.h>
#include <math.h>
/* Public Includes -----------------------------------------------------------*/
#include "main.h"

/* Private Includes ----------------------------------------------------------*/
#include "tim.h"
#include "timer_if.h"
#include "control.h"
#include "control_if.h"
/* Global variables ----------------------------------------------------------*/

/* Private variables ---------------------------------------------------------*/

/* Global functions ----------------------------------------------------------*/
/**
* @brief  transfer 	  raw information of encoder to joint angle
* @param  encoder  		info of joint incremental encoder
* @param  sbt					info os force sensor
* @param  joint       info of joint
* @retval none
*/
void parse_enc_data(SBT_FORCE* sbt, ENC_INC_INFO* encoder, JOINT_INFO* joint)
{
	/*  maxon incre angle */
	if(encoder->angle > 180)
		joint->inc[CURRENT] = encoder->angle - 360;
	else
		joint->inc[CURRENT] = encoder->angle;
	update_1th_filter(&joint->angle_fil, encoder->angle);
	
	/*  force sensor */
	joint->f_act = sbt->torque;
	update_1th_filter(&joint->f_fil,joint->f_act);
}

/**
* @brief  control motor by PWM and DIR
* @param  speed   desired output speed of motor (angle / min)
* @retval none
*/
#ifdef CURRENT_CONTROL
int motor_control(float force)
#else
int motor_control(float speed)
#endif
{
	int pwm;
	float control;

	const uint8_t dir_front = CLK; 
	
#ifdef CURRENT_CONTROL
	control = force/GEAR_RATIO/DCX22S_CURRENT2FORCE*DCX22S_CURREN_MAX;
#else
  control = speed*MIN2SEC/CYCLE/(DCX22S_SPEED_MAX/GEAR_RATIO);
#endif
	if(
		(control >= 0 && dir_front == CLK)
		 ||(control < 0 && dir_front == ANTICLK)
		)
	{
			pwm = (int)(fabs(control)*(900 - 500) + 500);
	}
	else
	{
			pwm = (int)(500 - fabs(control)*(500 - 100)); 
	}
	if (pwm < 100) 
	{
		 pwm = 100;				
	}	
	else if (pwm > 900) 
	{
		 pwm = 900;
	}			

	__HAL_TIM_SetCompare(&htim14,TIM_CHANNEL_1,pwm);

  return pwm;
}

/**
* @brief  control motor to search switch
* @param  speed   desired output speed of motor
* @retval none
*/
void motor_encoder_calibration(float speed, JOINT_INFO* joint)
{
	joint->speed_des[CURRENT] = speed;
	pid_speed_control(joint);
	motor_control(joint_info.f_out);
}
/**
* @brief  calculate max task cycle period
* @param  current_time  current time in us
* @retval maximum task cycle period
*/
float task_cycle(float current_time, uint8_t clear)
{
	static float last_time = 0, max_cycle_period = 0; 	
	if(((current_time - last_time)>=max_cycle_period)
	    &&(last_time !=0)
	)
	{
		max_cycle_period = current_time - last_time;
	}
	last_time = current_time;
	if(clear == 1)
	{
		max_cycle_period = 0;
	}
  return max_cycle_period;
}

void motor_enable(void)
{
	MOTOR_ENABLE();
	delay_us(10);
	__HAL_TIM_SetCompare(&htim14,TIM_CHANNEL_1,500);
	
}
void motor_disable(void)
{
	__HAL_TIM_SetCompare(&htim14,TIM_CHANNEL_1,500);
	MOTOR_DISABLE();
}

unsigned char error_detect(SBT_FORCE *sbt, ENC_INC_INFO *encoder, JOINT_INFO *joint)
{
	uint8_t error_code = 0;
	if(sbt->over_time_error >= 10)
		error_code |= 1 << 0;
	if(fabs(sbt->torque) > 1.0)
		error_code |= 1 << 1;
	#ifndef SPEED_CONTROL
	if(encoder->valid && fabs(joint->inc[CURRENT])>175)
		error_code |= 1 << 2;
	#endif
	return error_code;
}
/* Private functions ---------------------------------------------------------*/

/****************************** END OF FILE ***********************************/
